﻿#region Using Statements
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.Audio;
using System;
using System.Collections.Generic;

using Xeno_Race.Helpers;
using Xeno_Race.Graphics;
using Xeno_Race.SoundManager;
using System.Linq;
#endregion

namespace Xeno_Race.Game
{
    public class Ship
    {
        /// <summary>
        /// speed displayed in the hud
        /// </summary>
        private int speed = 0;

        /// <summary>
        /// Desired minimum height above ground
        /// </summary>
        private float minimumAltitude = 550.0f;

        /// <summary>
        /// Radius of ship bounding sphere, used for collisions
        /// </summary>
        private float radius = 200.0f;
        /// <summary>
        /// Radius of ship bounding sphere, used for collisions
        /// </summary>
        public float Radius
        {
            get { return radius; }
        }

        /// <summary>
        /// Location of ship in world space.
        /// </summary>
        public Vector3 Position, prevPosition;

        /// <summary>
        /// Direction ship is facing.
        /// </summary>
        public Vector3 Direction;

        private Vector3 right;
        /// <summary>
        /// Ship's right vector.
        /// </summary>
        public Vector3 Right
        {
            get { return right; }
        }
        /// <summary>
        /// Ship's up vector.
        /// </summary>
        public Vector3 Up;
        /// <summary>
        /// calculates speed and returns it
        /// </summary>
        /// 
        public int getSpeed()
        {
            speed = (int)this.Velocity.Length();
            speed = (speed - 4360) / 180;
            if (speed < 0) { speed = 0; }
            return speed;
        }

        /// <summary>
        /// Position the ship will start and be reset on
        /// </summary>
        public Vector3 StartPosition
        {
            get { return startPosition; }
            set 
            {
                startPosition.X = value.X;
                startPosition.Y = value.Y + minimumAltitude;
                startPosition.Z = value.Z;
            }
        }
        private Vector3 startPosition = new Vector3(0, 550.0f, 0);

        /// <summary>
        /// Direction in the ship will look at start and reset
        /// </summary>
        public Vector3 StartDirection
        {
            get { return startDirection; }
            set { startDirection =  value; }
        }
        private Vector3 startDirection = Vector3.Forward;

        /// <summary>
        /// Full speed at which ship can rotate; measured in radians per second.
        /// </summary>
        private const float RotationRate = 1.5f;

        /// <summary>
        /// Mass of ship.
        /// </summary>
        public float Mass = 80.0f;

        /// <summary>
        /// Maximum force that can be applied along the ship's direction.
        /// </summary>
        private float ThrustForce = 4000000.0f;

        /// <summary>
        /// Velocity scalar to approximate drag.
        /// </summary>
        private const float DragFactor = 0.97f;

        /// <summary>
        /// Current ship velocity.
        /// </summary>
        public Vector3 Velocity;

        /// <summary>
        /// Ship world transform matrix.
        /// </summary>
        public Matrix World
        {
            get { return world; }
        }
        private Matrix world;

        /// <summary>
        /// Sound of the ship engine, our ship has its own engine sound so we have exclusive
        /// access to it and can change it according to thrust.
        /// </summary>
        private SoundEffectInstance engineSound;

        /// <summary>
        /// Sets engine sound
        /// </summary>
        /// <returns>Model</returns>
        public SoundEffectInstance EngineSound
        {
            set { engineSound = value; }
        }

        /// <summary>
        /// Indicates if we are ingame and the ship is drawn or not
        /// </summary>
        private bool inGame;

        /// <summary>
        /// Setter/Getter for inGame value
        /// </summary>
        /// <returns>Bool</returns>
        public bool InGameStatus
        {
            get { return inGame; }
            set
            {
                inGame = value;
                // Shut off engine sound if we are not ingame
                if (!inGame)
                {
                    stopEngineSound();
                }
            }
        } // InGame

        /// <summary>
        /// Indicates if this is the second ship in multiplayer mode
        /// </summary>
        private bool secondShip;

        /// <summary>
        /// Setter/Getter for ship status
        /// </summary>
        /// <returns>Bool</returns>
        public bool SecondShip
        {
            get { return secondShip; }
            set { secondShip = value; }
        }

        /// <summary>
        /// Setter/Getter for Ship Model
        /// </summary>
        /// <returns>Model</returns>
        public Model UsedModel
        {
            get { return shipModel; }
            set { shipModel = value; }
        }

        /// <summary>
        /// The model this instance will use
        /// </summary>
        private Model shipModel;

        #region Vars for collision checks

        /// <summary>
        /// The interpolated points of our current track
        /// </summary>
        List<Vector3> extendedTrackPoints;

        /// <summary>
        /// Nearest trackpoint to our ship
        /// </summary>
        private int nearestTrackPoint = -1;

        /// <summary>
        /// Gets the current interpolated normal vector to the ships position
        /// </summary>
        /// <returns>Vector3</returns>
        public Vector3 InterpolatedNormal
        {
            get { return interpolatedNormal; }
        }
        /// <summary>
        /// The current interpolated normal vector to the ships position
        /// </summary>
        private Vector3 interpolatedNormal;

        /// <summary>
        /// Gets the current interpolated ship position
        /// </summary>
        /// <returns>Vector3</returns>
        public Vector3 InterpolatedPosition
        {
            get { return interpolatedPosition; }
        }
        /// <summary>
        /// Current interpolated ship position
        /// </summary>
        private Vector3 interpolatedPosition;

        /// <summary>
        /// Current direction of track/ship based on track points
        /// </summary>
        private Vector3 currShipDir;

        /// <summary>
        /// Current and minimum distance of ship to trackpoints
        /// </summary>
        private float currDist = 0, minDist = 0;

        /// <summary>
        /// Width of our track/2
        /// </summary>
        private float halfTrackWidth;

        /// <summary>
        ///  Amount to be rotated caused through wall collision
        /// </summary>
        private float rotateX;

        /// <summary>
        /// Gets if collisions are enabled
        /// </summary>
        /// <returns>bool</returns>
        public bool CollisionsEnabled
        {
            get { return collisions; }
        }
        /// <summary>
        ///  Enables or disables collision track checking
        /// </summary>
        private bool collisions = true;

        /// <summary>
        /// Saves the last track point we passed (required to check if we go in the right direction! :)
        /// </summary>
        private int pastTrackPoint = 0;

        /// <summary>
        /// Indicates if we are going along the direction of the track
        /// </summary>
        private bool forward = true;

        #endregion

        #region Initialization
        /// <summary>
        /// Default constructor, only requires track data
        /// </summary>
        public Ship(CollisionTrack currCollisionTrack)
        {
            // Set track data
            UpdateTrackData(currCollisionTrack);
        }

        /// <summary>
        /// Constructor for custmized ship including height above ground, thrust and mass
        /// </summary>
        public Ship(CollisionTrack currCollisionTrack, float minimumAltitude, float radius, float mass, float thrustForce)
        {
            // Set track data
            UpdateTrackData(currCollisionTrack);
            this.minimumAltitude = minimumAltitude;
            this.radius = radius;
            this.Mass = mass;
            this.ThrustForce = thrustForce;
        }

        /// <summary>
        /// Restore the ship to its original starting state
        /// </summary>
        public void Reset()
        {
            Position = startPosition;
            Direction = startDirection;
            Up = Vector3.Up;
            right = Vector3.Right;
            Velocity = Vector3.Zero;
            collisions = true;
            nearestTrackPoint = -1;
            pastTrackPoint = 0;
        }

        /// <summary>
        /// Initialises or updates the collision track data for collision checks.
        /// Will reset our ship also!
        /// </summary>
        public void UpdateTrackData(CollisionTrack currCollisionTrack)
        {
            // Get values from track
            extendedTrackPoints = currCollisionTrack.ExtendedTrackPoints;
            halfTrackWidth = currCollisionTrack.HalfTrackWidth;
            StartPosition = currCollisionTrack.TrackStartPoint;
            startDirection = currCollisionTrack.TrackStartDirection;
            startDirection.Normalize();

            checkpointIntervall = 48;
            float count = (currCollisionTrack.ExtendedTrackPoints.Count / checkpointIntervall);
            if (count <= 1.0f)
            {
                // Smaller intervall for smaller tracks!
                checkpointIntervall = 18;
                count = (currCollisionTrack.ExtendedTrackPoints.Count / checkpointIntervall);
            } 

            checkpointCount = ((int) Math.Floor(count)) + 1;
            CurrentLapCheckpoints = new bool[checkpointCount];

            Reset();
        }
        #endregion

        /// <summary>
        /// Stores array of trackpoints to determine whether all points have been passed.
        /// </summary>
        public bool[] CurrentLapCheckpoints { get; set; }

        /// <summary>
        /// After how many checkpoints should we check player progress
        /// </summary>
        private int checkpointIntervall = 48;

        /// <summary>
        /// Number of checkpoints
        /// </summary>
        private int checkpointCount;

        /// <summary>
        /// Applies a simple rotation to the ship and animates position based
        /// on simple linear motion physics.
        /// </summary>
        public void Update(GameTime gameTime)
        {
            prevPosition = Position;

            // TODO: Recode Input Handling to use our InputHelper Helper class!
            // InputHelper currentInputState = InputHelper.GetState();

            KeyboardState keyboardState = Keyboard.GetState();
            GamePadState gamePadState;

            if (secondShip)
                gamePadState = GamePad.GetState(PlayerIndex.Two);
            else
                gamePadState = GamePad.GetState(PlayerIndex.One);

            float elapsed = (float)gameTime.ElapsedGameTime.TotalSeconds;

            // Determine rotation amount from gamepad
            Vector2 rotationAmount = -gamePadState.ThumbSticks.Left;
            // Apply rotation inflicted by collision
            rotationAmount.X += rotateX;
            if (secondShip)
            {
                // Determine rotation amount from keyboard
                if (keyboardState.IsKeyDown(Keys.A))
                    rotationAmount.X = 1.0f;
                if (keyboardState.IsKeyDown(Keys.D))
                    rotationAmount.X = -1.0f;
                if (keyboardState.IsKeyDown(Keys.W))
                    rotationAmount.Y = -1.0f;
                if (keyboardState.IsKeyDown(Keys.S))
                    rotationAmount.Y = 1.0f;
            }
            else
            {
                // Determine rotation amount from keyboard
                if (keyboardState.IsKeyDown(Keys.Left))
                    rotationAmount.X = 1.0f;
                if (keyboardState.IsKeyDown(Keys.Right))
                    rotationAmount.X = -1.0f;
                if (keyboardState.IsKeyDown(Keys.Up))
                    rotationAmount.Y = -1.0f;
                if (keyboardState.IsKeyDown(Keys.Down))
                    rotationAmount.Y = 1.0f;
            }

            // Scale rotation amount to radians per second
            rotationAmount = rotationAmount * RotationRate * elapsed;

            // Correct the X axis steering when the ship is upside down
            if (Up.Y < 0)
                rotationAmount.X = -rotationAmount.X;

            // Create rotation matrix from rotation amount
            Matrix rotationMatrix =
                Matrix.CreateFromAxisAngle(Right, rotationAmount.Y) *
                Matrix.CreateRotationY(rotationAmount.X);

            // Rotate orientation vectors
            Direction = Vector3.TransformNormal(Direction, rotationMatrix);
            Up = Vector3.TransformNormal(Up, rotationMatrix);

            // Re-normalize orientation vectors
            // Without this, the matrix transformations may introduce small rounding
            // errors which add up over time and could destabilize the ship.
            Direction.Normalize();
            Up.Normalize();

            // Re-calculate Right
            right = Vector3.Cross(Direction, Up);

            // The same instability may cause the 3 orientation vectors may
            // also diverge. Either the Up or Direction vector needs to be
            // re-computed with a cross product to ensure orthagonality
            Up = Vector3.Cross(Right, Direction);


            // Determine thrust amount from input
            float thrustAmount = gamePadState.Triggers.Right;
            if (!secondShip && keyboardState.IsKeyDown(Keys.Space))
                thrustAmount = 1.0f;
            else if (secondShip && keyboardState.IsKeyDown(Keys.LeftControl))
                thrustAmount = 1.0f;

            // Calculate force from thrust amount
            Vector3 force = Direction * thrustAmount * ThrustForce;

            // Apply acceleration
            Vector3 acceleration = force / Mass;
            Velocity += acceleration * elapsed;

            // Apply psuedo drag
            Velocity *= DragFactor;

            float thrustUpAmount = gamePadState.Triggers.Left;
            if (!secondShip && keyboardState.IsKeyDown(Keys.X))
                thrustUpAmount = 1.0f;

            Vector3 forceUp = new Vector3(0, (thrustUpAmount * (98100.9f + ThrustForce * 1.0f)), 0);
            Vector3 accelerationUp = forceUp / Mass;
            Velocity += accelerationUp * elapsed;

            // Apply gravity
            Vector3 gravity = new Vector3(0, -1.0f, 0);
            gravity = gravity * 9.81f * 10.0f;
            Vector3 gravityForce = gravity * Mass;
            Velocity += gravityForce * elapsed;

            // Apply velocity
            Position += Velocity * elapsed;

            if (collisions)
            {
                // Do full collision checks and see if the player goes in the right direction!

                // Save our last trackpoint
                pastTrackPoint = nearestTrackPoint;

                // Get the currently nearest track point
                getNearestTrackPoint();

                // Did we get across a new trackpoint?
                if (pastTrackPoint != nearestTrackPoint)
                {
                    // Since c# doesn't behave as we expect it when we use modulo we have to 
                    // do a slightly different check in case we go from the very last to the
                    // first track point in the lap!
                    if (nearestTrackPoint == 0 && pastTrackPoint != 0 && pastTrackPoint != -1)
                    {
                        forward = (((nearestTrackPoint - pastTrackPoint) % extendedTrackPoints.Count) + extendedTrackPoints.Count) >= 1;
                    }
                    else
                    {
                        // Is the current trackpoint - past trackpoint greater than 1? If true we are doing it right :)
                        forward = (nearestTrackPoint - pastTrackPoint) >= 1;
                    }
                }

                // Tell the player if he is going in the wrong direction!
                if (!forward)
                    TextureFont.WriteText(1024 / 2 - 112, 30, "Wrong way!");
                else
                {
                    if (nearestTrackPoint < pastTrackPoint)
                    {
                        bool lapComplete = true;
                        foreach (bool trackPoint in CurrentLapCheckpoints)
                        {
                            if (trackPoint == false)
                            {
                                lapComplete = false;
                                break;// If a point has not been passed, the lap has not been finished, quit right away since there's no need to continue looking at the array
                            }
                        }
                        if (lapComplete)
                        {
                            // since the previous lap was finished, reset the CurrentLap array for the next lap
                            for (int i = 0; i < checkpointCount; i++)
                            {
                                CurrentLapCheckpoints[i] = false;
                            }
                            if(LapCompleted != null)
                                LapCompleted(this, EventArgs.Empty);
                        }
                    }

                    // TODO: Use current lap array to track lap progress
                    if ((nearestTrackPoint % checkpointIntervall) == 0)
                    {
                        CurrentLapCheckpoints[nearestTrackPoint/checkpointIntervall] = true;
                    }
                }

                // Collisions
                // Check the according groundplane
                checkGroundCollision();
                // Check according walls
                checkWallCollision();
            }
            else
            {
                // Simply prevent ship from flying under the ground
                rotateX = 0;
                Position.Y = Math.Max(Position.Y, minimumAltitude);
            }

            // Play engine sound according to amount of thrust, if no thrust, set sound to idle value
            if (thrustAmount > 0)
                playEngineSound(thrustAmount, thrustAmount);
            else
                playEngineSound(0.1f, 0.1f);

            // Reconstruct the ship's world matrix
            world = Matrix.Identity;
            world.Forward = Direction;
            world.Up = Up;
            world.Right = right;
            world.Translation = Position;
            
            // Display debug information
            //for (int i = 0; i < checkpointCount; i++)
            //{
            //    TextureFont.WriteText(248, 30*(i+1), "Checkpoint " + i + " : " + CurrentLapCheckpoints[i].ToString());
            //}
        }

        public event EventHandler LapCompleted;

        /// <summary>
        ///  Toggles collision track checking
        /// </summary>
        public void toggleCollisions()
        {
            collisions = !collisions;
        }

        #region Sounds
        private void playEngineSound(float volume, float pitch)
        {
            if (engineSound != null && engineSound.State == SoundState.Paused)
            {
                engineSound.Resume();
            }

            if (engineSound != null && engineSound.State == SoundState.Stopped)
            {
                engineSound.Play();
            }

            if (volume <= 0)
            {
                engineSound.Stop();
            }
            else
            {
                if (volume <= 0.1f)
                    volume = 0.1f;
                if (pitch <= 0.1f)
                    pitch = 0.1f;
                engineSound.Volume = volume*2;
                engineSound.Pitch = pitch/2;
            }
        }

        public void stopEngineSound()
        {
            engineSound.Stop();
        }
        #endregion

        public void moveStartPos(bool right)
        {
            Vector3 sideDir = Vector3.Cross(Vector3.Up, this.startDirection);
            sideDir.Normalize();

            if (right)
            {
                this.startPosition -= sideDir * (halfTrackWidth * 1/3);
            }
            else
            {
                this.startPosition += sideDir * (halfTrackWidth *  1/3);
            }
        }

        #region Collision Checks
        /*
         * Collision detection
         * We use a simple path with interpolated points as base for collision detection.
         * This path represents our track but will be invisible in the game, it is only used
         * for collision testing.
         * 
         * Priciple of collision checks to the ground and side walls:
         * 
         * (1) The first thing we do is to get the nearest track point to our ship, this is the 
         *     base point for all other calculations.
         * 
         * (2) We then check if we are behind or in front of this point
         * 
         * (3) Based on the position we use the previous or next track point for further calculations
         * 
         * (4) For interpolation we need to know the relative position of our ship in between the base
         *     point and the next/previous point
         *     
         * (5) As another value we need the two normal vectors at our points
         * 
         * (6) Now we use linear interpolation to get the exact position of our ship between the 
         *     points and the normal at this position
         *     
         * (7) Point and normal are the base for the plane wich acts as the current trackground,
         *     with wich we can now calculate the current height of the ship and see if it
         *     is above the minimum height and correct it if needed.
         *     
         * (8) After the floor collision has been done, we check if we are within the walls and
         *     adjust this position also and rotate the ship a bit to make it more realistic
         * 
         *  Note:
         *  Parts of code derived from: XnaRacingGame, TankOnAHeightmap-Example and from
         *  the book "XNA 2.0 Game Programming Recipes: A Problem-Solution Approach"
         *  
         */

        private void getNearestTrackPoint()
        {
            // Get the nearest track point by simply comparing distances
            nearestTrackPoint = -1;
            for (int i = 0; i < extendedTrackPoints.Count; i++)
            {
                currDist = Vector3.Distance(extendedTrackPoints[i], this.Position);
                if (currDist <= minDist || nearestTrackPoint == -1)
                {
                    nearestTrackPoint = i;
                    minDist = currDist;
                }
            }
        }

        private void checkGroundCollision()
        {
            // Get numbers of previous and next tp
            int previousTrackPoint = ((nearestTrackPoint - 1) % extendedTrackPoints.Count);
            if (previousTrackPoint < 0)
                previousTrackPoint += extendedTrackPoints.Count;
            int nextTrackPoint = ((nearestTrackPoint + 1) % extendedTrackPoints.Count);

            // Current Vector from nearest trackpoint to next
            currShipDir = extendedTrackPoints[nearestTrackPoint] - extendedTrackPoints[nextTrackPoint];
            // Get distance from our ship to current(nearest) trackpoint
            float distCurrToShip = signedDistanceToPlane(this.Position, extendedTrackPoints[nearestTrackPoint], currShipDir);
            // Check if we are behind or in front of the nearest trackpoint
            bool behindTP = distCurrToShip < 0;
            // Make dist absolute
            distCurrToShip = Math.Abs(distCurrToShip);

            float relativePosition = 0.5f;
            if(behindTP)
            {
                // Ship is behind the nearest trackpoint
                // Get distance and vector between ship and last trackpoint
                currShipDir = extendedTrackPoints[previousTrackPoint] - extendedTrackPoints[nearestTrackPoint];
                float distPrevToShip = distanceToPlane(this.Position, extendedTrackPoints[previousTrackPoint], currShipDir);
                relativePosition = distCurrToShip / (distCurrToShip + distPrevToShip);

                // Interpolate normal vector from current and last vector based on interpolated relative position
                interpolatedNormal = Vector3.Lerp(
                    getNormalVector(extendedTrackPoints[nearestTrackPoint],
                                    extendedTrackPoints[nextTrackPoint]),
                    getNormalVector(extendedTrackPoints[previousTrackPoint],
                                    extendedTrackPoints[nearestTrackPoint]),
                    relativePosition);

                // Interpolate absolute Position
                interpolatedPosition = Vector3.Lerp(
                    extendedTrackPoints[nearestTrackPoint],
                    extendedTrackPoints[previousTrackPoint],
                    relativePosition);
            }
            else
            {
                // Ship is in front the nearest trackpoint
                // Get distance between ship and next trackpoint, 
                // the according vector was already computed above!
                float distNextToShip = distanceToPlane(this.Position, extendedTrackPoints[nextTrackPoint], currShipDir);
                relativePosition = distCurrToShip / (distCurrToShip + distNextToShip);

                // Interpolate normal vector from current and next vector based on interpolated relative position
                interpolatedNormal = Vector3.Lerp(
                    getNormalVector(extendedTrackPoints[nearestTrackPoint], 
                                    extendedTrackPoints[nextTrackPoint]),
                    getNormalVector(extendedTrackPoints[nextTrackPoint], 
                                    extendedTrackPoints[(nextTrackPoint+1)%extendedTrackPoints.Count]),
                    relativePosition);

                // Interpolate absolute Position
                interpolatedPosition = Vector3.Lerp(
                    extendedTrackPoints[nearestTrackPoint],
                    extendedTrackPoints[nextTrackPoint],
                    relativePosition);
            }
            interpolatedNormal.Normalize();

            // Get height above current interpolated track plane
            float distToTrackFloor = distanceToPlane(
                this.Position, 
                interpolatedPosition,
                interpolatedNormal);

            // Adjust if we are too low above ground.
            // It doesn't matter if we are too high, gravity will pull us down
            if (distToTrackFloor < minimumAltitude)
                this.Position.Y += (minimumAltitude - distToTrackFloor);

            // Adjust rotation of ship to the current normal so it tilts according to ground
            // Will only applied if we are near above the ground, not when we fly!
            if ( distToTrackFloor < (minimumAltitude + (minimumAltitude * 0.1f)) )
            {
                this.Up = interpolatedNormal;
                //orientation.Up = normal;

                this.right = Vector3.Cross(this.Direction, this.Up);
                this.right = Vector3.Normalize(this.right);

                this.Direction = Vector3.Cross(this.Up, this.right);
                this.Direction = Vector3.Normalize(this.Direction);
            }
        }

        private void checkWallCollision()
        {
            // Now we check if we are too close to the sides, to do so we first need to
            // get vector perpendicular to the direction.
            // (To make things easier we do not interpolate it! May get rough in the corners though...)
            Vector3 sideDir = Vector3.Cross(currShipDir, interpolatedNormal);
            sideDir.Normalize();

            Vector3 rightWall = extendedTrackPoints[nearestTrackPoint] - sideDir * halfTrackWidth;
            Vector3 leftWall = extendedTrackPoints[nearestTrackPoint] + sideDir * halfTrackWidth;

            float rightDist = signedDistanceToPlane(this.Position, rightWall, sideDir);
            float leftDist = signedDistanceToPlane(this.Position, leftWall, sideDir);

            bool right = (leftDist + rightDist) >= 0;

            float angleToSide = MathHelper.ToDegrees(GetAngleBetweenVectors(this.Direction, sideDir));

            float relativeAngel = 0;
            this.rotateX = 0;

            if (right)
            {
                relativeAngel = 1 - ((angleToSide - 90) / 90);
                // If distance to right plane is positive we are outside!
                if (rightDist >= 0)
                {
                    if (relativeAngel > 0.2f && relativeAngel < 1.2f)
                    {
                        if (relativeAngel < 1.0f)
                            // Rotate this, the flatter the angle the faster the rotation, 
                            // but not more than 90°!
                            this.rotateX = relativeAngel / 2;
                        else
                            // Keep max acc below 1 otherwise we would speed up the this too fast!
                            relativeAngel = 1.0f;

                        // Slow this down based on collision angle
                        this.Velocity *= relativeAngel;

                        // Push this back on track, sidedir points from right to left!
                        this.Position += rightDist * sideDir;
                    }
                    // Vertical Hit, full stop
                    else if (relativeAngel > 0 && relativeAngel <= 0.2f)
                    {
                        this.Position += rightDist * sideDir;
                        this.Velocity = Vector3.Zero;
                    }
                    else
                    {
                        // For every other angle just push us back on track and slow us down a bit
                        this.Position += rightDist * sideDir;
                        this.Velocity *= 0.96f;
                    }
                }
            }
            else
            {
                relativeAngel = ((angleToSide) / 90);
                if (leftDist < 0)
                {
                    if (relativeAngel > 0.2f && relativeAngel < 1.2f)
                    {
                        if (relativeAngel < 1.0f)
                            // Rotate this, the flatter the angle the faster the rotation, 
                            // but not more than 90°!
                            this.rotateX = (0 - relativeAngel) / 2;
                        else
                            // Keep max acc below 1 otherwise we would speed up the ship too fast!
                            relativeAngel = 1.0f;

                        // Slow this down based on collision angle
                        this.Velocity *= relativeAngel;

                        // Push this back on track, sidedir points from right to left!
                        this.Position += leftDist * sideDir;
                    }
                    // Vertical Hit, full stop
                    else if (relativeAngel > 0 && relativeAngel <= 0.2f)
                    {
                        this.Position += leftDist * sideDir;
                        this.Velocity = Vector3.Zero;
                    }
                    else
                    {
                        // For every other angle just push us back on track and slow us down a bit
                        this.Position += leftDist * sideDir;
                        this.Velocity *= 0.96f;
                    }
                }
            }
        }
        #endregion

        #region Helper Methods
        /// <summary>
        /// Absolute distance to plane
        /// </summary>
        /// <param name="point">Point</param>
        /// <param name="planePosition">Plane position</param>
        /// <param name="planeNormal">Plane normal</param>
        /// <returns>Float</returns>
        public float distanceToPlane
            (Vector3 point, Vector3 planePosition, Vector3 planeNormal)
        {
            Vector3 pointVec = planePosition - point;
            return Math.Abs(Vector3.Dot(planeNormal, pointVec));
        }

        /// <summary>
        /// signed distance to plane
        /// </summary>
        /// <param name="point">Point</param>
        /// <param name="planePosition">Plane position</param>
        /// <param name="planeNormal">Plane normal</param>
        /// <returns>Float</returns>
        public float signedDistanceToPlane
            (Vector3 point, Vector3 planePosition, Vector3 planeNormal)
        {
            Vector3 pointVec = planePosition - point;
            return Vector3.Dot(planeNormal, pointVec);
        }

        /// <summary>
        /// Calculates normal vector from two points
        /// </summary>
        public Vector3 getNormalVector(Vector3 basePoint, Vector3 nextPoint)
        {
            Vector3 shipDir = nextPoint - basePoint;
            shipDir.Normalize();

            Vector3 currentNormal = Vector3.Up;
            Vector3 sideDir = Vector3.Cross(currentNormal, shipDir);
            sideDir.Normalize();

            currentNormal = Vector3.Cross(shipDir, sideDir);
            currentNormal.Normalize();

            return currentNormal;
        }

        /// <summary>
        /// Return angle between two vectors. Used for visbility testing and
        /// for checking angles between vectors
        /// </summary>
        /// <param name="vec1">Vector 1</param>
        /// <param name="vec2">Vector 2</param>
        /// <returns>Float</returns>
        public static float GetAngleBetweenVectors(Vector3 vec1, Vector3 vec2)
        {
            return (float)Math.Acos(Vector3.Dot(vec1, vec2));
        }
        #endregion
    }
}

